#include "../inc/comany.h"
#include <stdio.h>

#define UART_COMRECV "uart2"
#define COM_THREAD_PRIO 15
#define COM_THREAD_TICK 10
#define THREAD_STACK_SIZE 1024
#define COM_BUFFER_SIZE 64

#define ARG_NUM 9

static rt_device_t ComSerial;
static struct rt_semaphore rx_sem,command_sem;
char buffer[COM_BUFFER_SIZE];
rt_size_t buffer_len = 0;

static sendPackage_t package;
rt_mailbox_t package_mailbox;
rt_mailbox_t wave_data_mailbox;


rt_err_t uart_input_notice(rt_device_t dev, rt_size_t size)
{
    /* 串口接收到数据后产生中断，调用此回调函数，然后发送接收信号量 */
    rt_sem_release(&rx_sem);

    return RT_EOK;
}

void serial_thread_entry(void *parameter)
{
    char recv_ch;
    char *get_msg = "getted instruction\n";

    memset((void*)buffer,0,sizeof(buffer));

    while (1)
    {
        while (rt_device_read(ComSerial, -1, &recv_ch, 1) != 1)
        {
            rt_sem_take(&rx_sem, RT_WAITING_FOREVER);
        }

        if(recv_ch == '\n')
        {

            rt_device_write(ComSerial, 0, get_msg, strlen(get_msg));
            //memset((void*)buffer,0,COM_BUFFER_SIZE);
            rt_sem_release(&command_sem);       //通知命令解析线程进行命令解析
        }
        else
        {
            rt_enter_critical();

            buffer[buffer_len] = recv_ch;
            buffer_len += 1;
            rt_exit_critical();
        }

    }
}

/*
 * 函数作用:用于解析指令
     * 指令格式   name:value
 *
 * */

void command_anys_thread(void *parameter)
{
    /*
         * 指令格式   name:value
     *
     * */

    //rt_device_pulse_encoder_register(pulse_encoder, name, user_data)
    char command_name[8];
    char command_value[8];
    rt_size_t cpy_point = 0;
    rt_size_t cpy_v_point = 0;

    char * ok_msg = "set value successfully!";
    char * no_msg = "set value failed!";


    memset((void*)command_name,'\0',sizeof(buffer));
    memset((void*)command_value,'\0',sizeof(buffer));


    while(1)
    {
        rt_sem_take(&command_sem,RT_WAITING_FOREVER);

        rt_enter_critical();
        for(cpy_point = 0;cpy_point < buffer_len;++cpy_point)           //提取变量名
        {
            if(buffer[cpy_point] == ':'){
                cpy_point += 1;
                break;
            }
            command_name[cpy_point] = buffer[cpy_point];
        }

        //rt_device_write(ComSerial, 0, command_name,rt_strlen(command_name));

        for(cpy_v_point = 0;cpy_point < buffer_len;cpy_point++,cpy_v_point++)   // 提取变量值
        {
            command_value[cpy_v_point] = buffer[cpy_point];
        }
        //rt_device_write(ComSerial, 0, command_value, rt_strlen(command_value));

        if(setv(command_name,command_value) != -1){
            rt_device_write(ComSerial, 0, ok_msg, rt_strlen(ok_msg));
            rt_mb_send(package_mailbox, (rt_ubase_t)&package);
        }
        else{
            rt_device_write(ComSerial, 0, no_msg, rt_strlen(no_msg));
        }

        buffer_len = 0;
        cpy_point = 0;
        cpy_v_point = 0;
        memset((void*)buffer,'\0',sizeof(buffer));
        memset((void*)command_name,'\0',sizeof(command_name));
        memset((void*)command_value,'\0',sizeof(command_value));

        rt_exit_critical();
    }

}

int8_t setv(char *name,char * value)
{
    char *name_list[ARG_NUM] = {"kp","kd","ki","mo_num","vx","vy","vz","tg_encoder","wave"};
    int8_t point = -1;
 //   char sendmsg[64] = {0};


    for(int i = 0;i < ARG_NUM;++i)
    {
        if(!strcmp(name_list[i],name))
        {
            point = i;
            break;
        }
    }

    switch(point)
    {
    case -1:
        break;
    case 0:
        package.kp = atof(value);
        break;
    case 1:
        package.ki = atof(value);
        break;
    case 2:
        package.kd = atof(value);
        break;
    case 3:
        package.m_n = atoi(value);
        break;
    case 4:
        package.v_x = atof(value);
        break;
    case 5:
        package.v_y = atof(value);
        break;
    case 6:
        package.v_z = atof(value);
        break;
    case 7:
        package.target_encoder = atof(value);
        break;
    case 8:
        package.startup_send_wave = atoi(value);

        break;

    }

    //package.v_x = 0.5;
//    rt_sprintf(sendmsg,"Hey:%d\n",package.m_n);
//    rt_kprintf("Hey:%d\n",&package.m_n);
//    rt_device_write(ComSerial, 0, sendmsg, strlen(sendmsg));

    return point;
}
/*
 * 函数功能: 波形发送线程，用于发送波形
 *利用mailbox
 *
 * */
void wave_send_thread(void *parameter)
{
    char send_wave[32];

    while(1)
    {
        if(package.startup_send_wave == 1)
        {
            //rt_mb_recv(wave_data_mailbox,(rt_ubase_t*)&send_wave_data,RT_WAITING_FOREVER);

            //rt_kprintf("This is here \n\r");

            rt_enter_critical();
//            rt_sprintf(send_wave,"w:%d,%d,%d,%d \r\n",wave_data.encoder_data[0],wave_data.encoder_data[1],
//                    wave_data.encoder_data[2],wave_data.encoder_data[3]);
            rt_sprintf(send_wave,"e:%d,%d,%d \r\n",wave_data.pitch,wave_data.yaw,wave_data.roll);
            rt_device_write(ComSerial, 0, send_wave, strlen(send_wave));

            memset((void *)&send_wave,'\0',sizeof(send_wave));
            rt_exit_critical();

        }
    }
}

/*
 * 串口初始化
 * 启动串口指令处理线程
 *
 * */
rt_err_t uart_init(void)
{
    ComSerial = rt_device_find(UART_COMRECV);
    char *send_msg = "Welcoming to use s1-uart-debug";
    if (!ComSerial)
    {
        rt_kprintf("find %s failed!\n", uart_input_notice);
        return RT_ERROR;
    }

    package_mailbox = rt_mb_create("update_value_mailbox", 1, RT_IPC_FLAG_FIFO);
    wave_data_mailbox = rt_mb_create("wave date send box", 1, RT_IPC_FLAG_FIFO);

    rt_sem_init(&rx_sem, "rx_sem", 0, RT_IPC_FLAG_FIFO);
    rt_sem_init(&command_sem,"command_sem",0,RT_IPC_FLAG_FIFO);     //指令处理信号量,处理线程等待指令接收完成

    rt_device_open(ComSerial, RT_DEVICE_OFLAG_RDWR | RT_DEVICE_FLAG_INT_RX);
    rt_device_set_rx_indicate(ComSerial, uart_input_notice);
    rt_device_write(ComSerial, 0, send_msg, strlen(send_msg));

    rt_thread_t thread = rt_thread_create("serial", serial_thread_entry, RT_NULL, 1024, COM_THREAD_PRIO, COM_THREAD_TICK);
    if (thread != RT_NULL)
    {
        rt_kprintf("Create uart-recv thread sucessfully!\r\n");
        rt_thread_startup(thread);
    }
    else
    {
        rt_kprintf("Create uart-recv thread failed!\r\n");
        return RT_ERROR;
    }


    /*命令解析函数优先级比接收优先级低一点*/
    rt_thread_t com_thread = rt_thread_create("command analyze", command_anys_thread, RT_NULL, 1024, COM_THREAD_PRIO+1, COM_THREAD_TICK);
    if (com_thread != RT_NULL)
    {
        rt_kprintf("Create com_thread thread sucessfully!\r\n");
        rt_thread_startup(com_thread);
    }
    else
    {
        rt_kprintf("Create com_thread  failed!\r\n");
        return RT_ERROR;
    }

    rt_thread_t wave_thread = rt_thread_create("wave send thread", wave_send_thread, RT_NULL, 1024, COM_THREAD_PRIO+5, COM_THREAD_TICK);
    if (wave_thread != RT_NULL)
    {
        rt_kprintf("Create wave_thread thread sucessfully!\r\n");
        rt_thread_startup(wave_thread);
    }
    else
    {
        rt_kprintf("Create wave_thread  failed!\r\n");
        return RT_ERROR;
    }

    rt_kprintf("UART Handle thread startup\r\n");
    return RT_EOK;
}
MSH_CMD_EXPORT(uart_init,command recv uart init);
